#ifndef MAP_PROXY_STATE_VALIDITY_CHECKER_H
#define MAP_PROXY_STATE_VALIDITY_CHECKER_H

#include <iostream>
#include <set>
#include <libplayerc++/playerc++.h>
#include "ompl_includes.h"
#include "PlayerObject.h"
#include "PlayerObjectManager.h"

typedef ompl::base::SE2StateSpace::StateType SE2StateType;

class MapProxyStateValidityChecker: public ompl::base::StateValidityChecker {


	double radius; // radius of robot in metres

	// represents a cell in the map.
	struct cell {
		int x;
		int y;

		cell(int x, int y) : x(x), y(y) {}

		cell(const cell &other) : x(other.x), y(other.y) {}

		cell & operator=(const cell &other) {
			x = other.x;
			y = other.y;
			return *this;
		}

		bool operator==(const cell &other) const {
			return x == other.x && y == other.y;
		}

		const cell operator+(const cell &other) const {
			cell result(*this);
			result.x += other.x;
			result.y += other.y;
			return result;
		}

		bool operator<(const cell &other) const {
			if (x == other.x) return y < other.y;
			return x < other.x;
		}

	};

	friend std::ostream & operator<<(std::ostream &, const cell &);

	const PlayerCc::MapProxy &map;
	double resolution;
	uint32_t width;
	uint32_t height;
	double originX;
	double originY;
	int8_t dataRange;
	std::set<cell> bounds; // cells in bounding circle, from origin (0,0)

	// compute circle from origin(0,0)
	// adapted from http://en.wikipedia.org/wiki/Midpoint_circle_algorithm#Examples
	static std::set<cell> computeCircle(int radius) {
		std::set<cell> bounds;
		int x = radius, y = 0;
		int xChange = 1 - (radius << 1);
		int yChange = 0;
		int radiusError = 0;
 
		while(x >= y)
		{
			bounds.insert(cell(x, y));
			bounds.insert(cell(y, x));
			bounds.insert(cell(-x, y));
			bounds.insert(cell(-y, x));
			bounds.insert(cell(-x, -y));
			bounds.insert(cell(-y, -x));
			bounds.insert(cell(x, -y));
			bounds.insert(cell(y, -x));

			y++;
			radiusError += yChange;
			yChange += 2;
			if(((radiusError << 1) + xChange) > 0)
			{
				x--;
				radiusError += xChange;
				xChange += 2;
			}
		}
		return bounds;
	}

	int cellX(double x) const {
		return static_cast<int>((x - originX) / resolution);
	}

	int cellY(double y) const {
		return static_cast<int>((y - originY) / resolution);
	}

	bool isCellValid(int dx, int dy) const {
		if (dx < 0 || dx >= (int)width) return false;
		if (dy < 0 || dy >= (int)height) return false;
		int cellValue = map.GetCell(dx, dy);
		return cellValue < 0;
	}

	bool isCellValid(cell c) const {
		return isCellValid(c.x, c.y);
	}

	bool isPointValid(double x, double y) const {
		int dx = cellX(x);
		int dy = cellY(y);
		return isCellValid(dx, dy);
	}

	bool isCircleValid(double x, double y) const {
		for (std::set<cell>::iterator i = bounds.begin(); i != bounds.end(); ++i) {
			cell p(*i);
			cell q(cellX(x), cellY(y));
			cell r = p + q;
			if (!isCellValid(r)) { return false; }
		}
		// all checked out
		return true;
	}

public:
	MapProxyStateValidityChecker(
		double radius, // radius of robot in metres
		const ompl::base::SpaceInformationPtr & space,
		const PlayerCc::MapProxy &map // map with loaded data
		) : radius(radius), ompl::base::StateValidityChecker(space), map(map) {
		resolution = map.GetResolution();
		width = map.GetWidth();
		height = map.GetHeight();
		originX = map.GetOriginX();
		originY = map.GetOriginY();
		int cellRadius = static_cast<int>(ceil(radius / resolution));
		bounds = computeCircle(cellRadius);
		if (cellRadius <= 0 || bounds.size() <= 0) {
			throw std::invalid_argument("check mapfile");
		}
		std::cout << "cell radius " << cellRadius
			<< "\nbounds of size " << bounds.size() << std::endl;
	}

	/*
		State is SE2 (x, y, yaw). (x, y) in metres.
	*/
	virtual bool isValid(const ompl::base::State * state) const {

		// get state details
		const SE2StateType * se2state = state->as<SE2StateType>();
		double x = se2state->getX();
		double y = se2state->getY();
		double yaw = se2state->getYaw();

        if ( !si_->satisfiesBounds(se2state) )
        {
            return false;
        }

        const std::vector<PlayerObject>& vecObjects = PlayerObjectManager::Instance().GetStaticObjects();
        for ( std::vector<PlayerObject>::const_iterator itLoop = vecObjects.begin(); itLoop != vecObjects.end(); ++itLoop )
        {
            if ( itLoop->isCollided( x, y, 0.4 ))
            {
                return false;
            }
        }
        
		return isPointValid(x, y) && isCircleValid(x, y);
	}
};

inline
std::ostream & operator<<(std::ostream &o, const MapProxyStateValidityChecker::cell & c) {
	o << "cell(" << c.x << ", " << c.y << ")";
	return o;
}

#endif
